﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Engine.Physics;
using Engine.Graphics;
using Microsoft.Xna.Framework;

namespace Engine.Physics
{
    public struct CollisionPair
    {
        public Collider Coll1;
        public Collider Coll2;

        public CollisionPair(Collider _coll1, Collider _coll2)
        {
            Coll1 = _coll1;
            Coll2 = _coll2;
        }

        public override bool Equals(object obj)
        {
            return (Coll1 == ((CollisionPair)obj).Coll1 && Coll2 == ((CollisionPair)obj).Coll2) || (Coll2 == ((CollisionPair)obj).Coll1 && Coll1 == ((CollisionPair)obj).Coll2);
        }
    }

    public abstract class BroadPhaseAlgorithm
    {
        public virtual List<CollisionPair> FindPairs(List<Collider> _colliders)
        {
            return new List<CollisionPair>();
        }

        public virtual void NotifyColliderAdded(List<Collider> _colliders)
        {

        }

        public virtual void NotifyColliderRemoved(List<Collider> _colliders)
        {

        }
    }

    public class AABBTree : BroadPhaseAlgorithm
    {
        class AABBNode
        {
            public AABBNode A;
            public AABBNode B;
            List<Collider> leaf;
            AABBNode Parent;
            public Rectangle bb;

            public AABBNode(AABBNode _parent, AABBNode _a, AABBNode _b, List<Collider> _leaf = null)
            {
                Parent = _parent;
                A = _a;
                B = _b;
                if (_leaf != null)
                    bb = FindSmallestAABB(_leaf);
                leaf = _leaf;
            }

            public void FindPossibleIntersections(Collider _coll, ref List<Collider> _intersections)
            {
                if (A != null && A.bb.Intersects(_coll.AABB))
                    A.FindPossibleIntersections(_coll, ref _intersections);

                if (B != null && B.bb.Intersects(_coll.AABB))
                    B.FindPossibleIntersections(_coll, ref _intersections);

                if (leaf != null)
                    _intersections.AddRange(leaf);
            }

            public void FindAllPairs(ref List<CollisionPair> _pairs)
            {
                if (A != null)
                    A.FindAllPairs(ref _pairs);

                if (B != null)
                    B.FindAllPairs(ref _pairs);

                if (leaf != null)
                    for (int i = 0; i < leaf.Count-1; i++)
                        for (int j = i+1; j < leaf.Count; j++)
                            _pairs.Add(new CollisionPair(leaf[i], leaf[j]));
            }

            public void Draw()
            {
                Rectangle drawRect = bb;
                if (GraphicsEngine.Instance.MainCamera != null)
                    drawRect = GraphicsEngine.Instance.MainCamera.WorldToScreen(bb);
                GraphicsEngine.Instance.DrawGizmo(drawRect);                
                if (A != null)
                    A.Draw();
                if (B != null)
                    B.Draw();
            }
        }

        List<CollisionPair> pairs = new List<CollisionPair>();
        List<Collider> otherColls = new List<Collider>();
        AABBNode root;
        public AABBTree(List<Collider> _colliders)
        {
            BuildNodeFromColliders(ref root, null, _colliders);
        }

        public AABBTree()
        {
        }

        public override List<CollisionPair> FindPairs(List<Collider> _colliders)
        {
            pairs.Clear();
            /*foreach (Collider coll in _colliders)
            {
                otherColls.Clear();
                root.FindPossibleIntersections(coll, ref otherColls);
                foreach (Collider other in otherColls)
                {
                    if (other != coll)
                    {
                        CollisionPair newPair = new CollisionPair(coll, other);
                        if(!pairs.Contains(newPair))
                            pairs.Add(newPair);
                    }
                }
            }

            List<CollisionPair> pairs2 = new List<CollisionPair>();*/
            root.FindAllPairs(ref pairs);
            //Graphics.GraphicsEngine.Instance.DrawString(pairs2.Count.ToString(), new Vector2(20, 40), Color.White);
            return pairs;
        }

        public override void NotifyColliderAdded(List<Collider> _colliders)
        {
            root = null;
            BuildNodeFromColliders(ref root, null, _colliders);
        }

        public override void NotifyColliderRemoved(List<Collider> _colliders)
        {
            root = null;
            BuildNodeFromColliders(ref root, null, _colliders);
        }

        public void Draw()
        {
            if(root != null)
                root.Draw();
        }

        void BuildNodeFromColliders(ref AABBNode _node, AABBNode _parent, List<Collider> _colliders)
        {
            if(_colliders.Count == 1)
                _node = new AABBNode(_parent, null, null, _colliders);
            else
            {
                List<Collider> aColls = new List<Collider>(_colliders.Count);
                List<Collider> bColls = new List<Collider>(_colliders.Count);

                _node = new AABBNode(_parent, null, null, null);
                _node.bb = FindSmallestAABB(_colliders);

                Rectangle aBB;
                Rectangle bBB;

                SplitAABB(_node.bb, out aBB, out bBB);

                foreach (Collider coll in _colliders)
                {
                    if (coll.AABB.Intersects(aBB))
                        aColls.Add(coll);
                    if (coll.AABB.Intersects(bBB))
                        bColls.Add(coll);
                }


                if (aColls.Count == _colliders.Count)
                    _node.A = new AABBNode(_node, null, null, aColls);
                else if (aColls.Count > 0)
                    BuildNodeFromColliders(ref _node.A, _node, aColls);

                if (bColls.Count == _colliders.Count)
                    _node.B = new AABBNode(_node, null, null, bColls);
                else if (bColls.Count > 0)
                    BuildNodeFromColliders(ref _node.B, _node, bColls);
            }
        }

        void SplitAABB(Rectangle _original, out Rectangle _a, out Rectangle _b)
        {
            if (_original.Width > _original.Height) //Split vertically
            {
                _a = new Rectangle(_original.X, _original.Y, _original.Width / 2, _original.Height);
                _b = new Rectangle(_original.X + _original.Width / 2, _original.Y, _original.Width / 2, _original.Height);
            }
            else //Split horizontally
            {
                _a = new Rectangle(_original.X, _original.Y, _original.Width, _original.Height / 2);
                _b = new Rectangle(_original.X, _original.Y + _original.Height / 2, _original.Width, _original.Height / 2);
            }
        }

        public static Rectangle FindSmallestAABB(List<Collider> _colliders)   
        {
            int minX = int.MaxValue;
            int minY = int.MaxValue;
            int maxX = int.MinValue;
            int maxY = int.MinValue;

            foreach (Collider coll in _colliders)
            {
                if (coll.AABB.Left < minX)
                    minX = coll.AABB.Left;

                if (coll.AABB.Top < minY)
                    minY = coll.AABB.Top;

                if (coll.AABB.Right > maxX)
                    maxX = coll.AABB.Right;

                if (coll.AABB.Bottom > maxY)
                    maxY = coll.AABB.Bottom;
            }

            return new Rectangle(minX, minY, maxX - minX, maxY - minY);
        }
    }

    public class SweepAndPrune : BroadPhaseAlgorithm
    {
        public override List<CollisionPair> FindPairs(List<Collider> _colliders)
        {
            if (_colliders.Count == 0)
                return new List<CollisionPair>();
            SortColliders(_colliders);

            List<CollisionPair> pairs = new List<CollisionPair>(_colliders.Count);

            List<Collider> activeColliders = new List<Collider>();

            for (int i = 0; i < _colliders.Count; i++)
            {
                if (activeColliders.Count > 0)
                {
                    for (int j = activeColliders.Count - 1; j >= 0; j--)
                        if (activeColliders[j].AABB.Right < _colliders[i].AABB.Left)
                            activeColliders.RemoveAt(j);

                    foreach (Collider coll in activeColliders)
                        pairs.Add(new CollisionPair(coll, _colliders[i]));
                }
                activeColliders.Add(_colliders[i]);               
            }
            return pairs;
        }

        void SortColliders(List<Collider> _colliders)
        {
            //Insertion Sort
            for (int i = 1; i < _colliders.Count; i++)
            {
                Collider low = _colliders[i];
                int j = i;

                while (j > 1 && _colliders[j - 1].AABB.Left > low.AABB.Left)
                {
                    _colliders[j] = _colliders[j - 1];
                    j--;
                }
                _colliders[j] = low;
            }
        }
    }
}
